﻿///////////////////////////////////////////////////////////////////////////////
// Activity: program.activity2
// MoveJoint service implementation
///////////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;

using ccr = Microsoft.Ccr.Core;
using dss = Microsoft.Dss.Core;
using dssa = Microsoft.Dss.Core.Attributes;
using dssh = Microsoft.Dss.Core.DsspHttp;
using dssm = Microsoft.Dss.ServiceModel.DsspServiceBase;
using dssp = Microsoft.Dss.ServiceModel.Dssp;
using soap = W3C.Soap;

using submgr = Microsoft.Dss.Services.SubscriptionManager;
using articulatedarm = Microsoft.Robotics.Services.ArticulatedArm.Proxy;
using sound = Microsoft.Robotics.Services.Sample.Sound.Proxy;
using timer = Microsoft.Robotics.Services.Sample.Timer.Proxy;

namespace Robotics.MoveArm_Sim2.MoveJoint
{
    [DisplayName("MoveArm_Sim2MoveJoint")]
    [Description("A user defined activity.")]
    [dssa.Contract(Contract.Identifier)]
    public class MoveJointService : dssm.DsspServiceBase
    {
        // Service state
        [dssa.InitialStatePartner(Optional = true)]
        private MoveJointState _state;

        // Service operations port
        [dssa.ServicePort("/MoveArm_Sim2/MoveJoint", AllowMultipleInstances = false)]
        private MoveJointOperations _mainPort = new MoveJointOperations();

        #region Partner services

        [dssa.Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = dssa.PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _subMgr = new submgr.SubscriptionManagerPort();

        // Partner: SoundPlayer, Contract: http://schemas.microsoft.com/robotics/2006/08/sound.html
        [dssa.Partner("SoundPlayer", Contract = sound.Contract.Identifier, CreationPolicy = dssa.PartnerCreationPolicy.UsePartnerListEntry)]
        sound.SoundOperations _soundPlayerPort = new sound.SoundOperations();

        // Partner: Timer, Contract: http://schemas.microsoft.com/robotics/2006/08/timer.html
        [dssa.Partner("Timer", Contract = timer.Contract.Identifier, CreationPolicy = dssa.PartnerCreationPolicy.UsePartnerListEntry)]
        timer.TimerOperations _timerPort = new timer.TimerOperations();

        // Partner: LynxL6Arm, Contract: http://schemas.microsoft.com/2006/06/articulatedarm.html
        [dssa.Partner("LynxL6Arm", Contract = articulatedarm.Contract.Identifier, CreationPolicy = dssa.PartnerCreationPolicy.UsePartnerListEntry)]
        articulatedarm.ArticulatedArmOperations _lynxL6ArmPort = new articulatedarm.ArticulatedArmOperations();

        #endregion

        public MoveJointService(dssp.DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        protected override void Start()
        {
            // If there was no initial state partner, then the service state will be null.
            if (_state == null)
            {
                // The state MUST be created before the service starts processing messages.
                _state = new MoveJointState();
            }

            // The rest of the start process requires the ability to wait for responses from
            // services and from the start handler (if any). So execution now proceeds in an
            // iterator function.
            SpawnIterator(DoStart);
        }

        private IEnumerator<ccr.ITask> DoStart()
        {

            // Start operation handlers and insert into directory service.
            StartHandlers();

            yield break;
        }

        private void StartHandlers()
        {
            // Activate message handlers for this service and insert into the directory.
            base.Start();
        }

        #region Standard DSS message handlers

        [dssa.ServiceHandler(dssa.ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ccr.ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        [dssa.ServiceHandler(dssa.ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ccr.ITask> HttpGetHandler(dssh.HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new dssh.HttpResponseType(_state));
            yield break;
        }

        [dssa.ServiceHandler(dssa.ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ccr.ITask> ReplaceHandler(Replace replace)
        {
            _state = replace.Body;

            replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
            base.SendNotification<Replace>(_subMgr, replace);

            yield break;
        }

        [dssa.ServiceHandler(dssa.ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ccr.ITask> SubscribeHandler(Subscribe subscribe)
        {
            dssp.SubscribeRequestType request = subscribe.Body;

            yield return ccr.Arbiter.Choice(
                SubscribeHelper(_subMgr, request, subscribe.ResponsePort),
                delegate(ccr.SuccessResult success)
                {
                    base.SendNotification<Replace>(_subMgr, request.Subscriber, _state);
                },
                delegate(Exception e) { }
            );
        }

        #endregion

        #region Custom message handlers

        [dssa.ServiceHandler(dssa.ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ccr.ITask> JointParamsHandler(JointParams message)
        {
            JointParamsMessageHandler handler = new JointParamsMessageHandler(this, Environment.TaskQueue);
            return handler.RunHandler(message.Body, message.ResponsePort);
        }

        #endregion

        #region JointParamsMessageHandler class

        class JointParamsMessageHandler : HandlerBase
        {
            ///////////////////////////////////////////////////////////////////
            // __use__3.snippet.snippet.noop - [(__use__3.snippet.snippet.snippet0.noop - [(__use__3.snippet.snippet.snippet0.expr0 - __use__3.snippet.snippet.snippet0.join)(__use__3.snippet.snippet.snippet0.snippet.noop - [(__use__3.snippet.snippet.snippet0.snippet.expr1 - __use__3.snippet.snippet.snippet0.snippet.join)(__use__3.snippet.snippet.snippet0.snippet.expr0 - &__use__3.snippet.snippet.snippet0.snippet.join)(__use__3.snippet.snippet.snippet0.snippet.expr - &__use__3.snippet.snippet.snippet0.snippet.join)] - &__use__3.snippet.snippet.snippet0.join)] - __use__3.snippet.snippet.join)(__use__3.snippet.snippet.snippet.noop - [(__use__3.snippet.snippet.snippet.expr1 - __use__3.snippet.snippet.snippet.join)(__use__3.snippet.snippet.snippet.expr0 - &__use__3.snippet.snippet.snippet.join)(__use__3.snippet.snippet.snippet.expr - &__use__3.snippet.snippet.snippet.join)] - &__use__3.snippet.snippet.join)(__use__3.snippet.snippet.expr - &__use__3.snippet.snippet.join)] - __use__3.snippet.call - __use__3.snippet.call.iftype
            // [(__use__1.snippet0.snippet.noop - __use__1.snippet0.snippet.expr - __use__1.snippet0.snippet.join)] - __use__1.snippet0.call - __use__1.snippet0.call.iftype
            // [(__use__2.snippet.snippet.noop - __use__2.snippet.snippet.expr - __use__2.snippet.snippet.join)] - __use__2.snippet.call - __use__2.snippet.call.iftype
            // expr - element
            // snippet.snippet.noop - snippet.snippet.expr - snippet.snippet.join
            // snippet.element
            // expr0 - &element
            ///////////////////////////////////////////////////////////////////

            public JointParamsMessageHandler(MoveJointService service, ccr.DispatcherQueue queue)
                : base(service, queue)
            {

                // Activate merge handlers
                Activate(ccr.Arbiter.Receive(true, _mergeAlpha, _mergeAlphaHandler));

                // Register joins with base class to manage active count for incomplete joins.
                base.RegisterJoin(_joinAlphaPorts);
                // Activate Join handlers
                Activate(ccr.Arbiter.MultiplePortReceive(true, _joinAlphaPorts, _joinAlphaHandler));
            }

            dssp.DsspResponsePort<JointParamsResponse> _responsePort;

            ///////////////////////////////////////////////////////////////////
            // __use__3.snippet.snippet.noop - [(__use__3.snippet.snippet.snippet0.noop - [(__use__3.snippet.snippet.snippet0.expr0 - __use__3.snippet.snippet.snippet0.join)(__use__3.snippet.snippet.snippet0.snippet.noop - [(__use__3.snippet.snippet.snippet0.snippet.expr1 - __use__3.snippet.snippet.snippet0.snippet.join)(__use__3.snippet.snippet.snippet0.snippet.expr0 - &__use__3.snippet.snippet.snippet0.snippet.join)(__use__3.snippet.snippet.snippet0.snippet.expr - &__use__3.snippet.snippet.snippet0.snippet.join)] - &__use__3.snippet.snippet.snippet0.join)] - __use__3.snippet.snippet.join)(__use__3.snippet.snippet.snippet.noop - [(__use__3.snippet.snippet.snippet.expr1 - __use__3.snippet.snippet.snippet.join)(__use__3.snippet.snippet.snippet.expr0 - &__use__3.snippet.snippet.snippet.join)(__use__3.snippet.snippet.snippet.expr - &__use__3.snippet.snippet.snippet.join)] - &__use__3.snippet.snippet.join)(__use__3.snippet.snippet.expr - &__use__3.snippet.snippet.join)] - __use__3.snippet.call - __use__3.snippet.call.iftype
            ///////////////////////////////////////////////////////////////////

            public IEnumerator<ccr.ITask> RunHandler(JointParamsRequest message, dssp.DsspResponsePort<JointParamsResponse> responsePort)
            {
                _responsePort = responsePort;
                Increment();

                JoinBeta a = new JoinBeta();
                a.Z = 0;
                a.Y = 0;
                a.X = 1;
                JoinGamma b = new JoinGamma();
                b.Angle = message.Angle;
                b.Axis = a;
                JoinBeta c = new JoinBeta();
                c.Z = 0;
                c.Y = 0;
                c.X = 0;
                articulatedarm.SetJointTargetPoseRequest request = new articulatedarm.SetJointTargetPoseRequest();
                request.TargetOrientation = (Microsoft.Robotics.PhysicalModel.Proxy.AxisAngle)b;
                request.TargetPosition = (Microsoft.Robotics.PhysicalModel.Proxy.Vector3)c;
                request.JointName = message.JointName;

                Increment();
                Activate(
                    ccr.Arbiter.Choice(
                        LynxL6ArmPort.SetJointTargetPose(request),
                        OnSetJointTargetPoseSuccess,
                        OnSetJointTargetPoseFault
                    )
                );

                Decrement();

                yield return WaitUntilComplete();
            }

            ///////////////////////////////////////////////////////////////////
            // snippet.element
            ///////////////////////////////////////////////////////////////////

            void _joinAlphaHandler(object[] args)
            {
                JoinAlpha message = new JoinAlpha(args);
                _responsePort.Post(new JointParamsResponse());

                Decrement(args.Length);
            }

            void OnSetJointTargetPoseSuccess(dssp.DefaultUpdateResponseType response)
            {

                Increment();
                _mergeAlpha.Post(500);

                Decrement();
            }

            void OnSetJointTargetPoseFault(soap.Fault response)
            {
                sound.BeepRequest request = new sound.BeepRequest();

                Increment();
                Activate(
                    ccr.Arbiter.Choice(
                        SoundPlayerPort.Beep(request),
                        OnBeepSuccess,
                        delegate(soap.Fault fault)
                        {
                            base.FaultHandler(fault, @"SoundPlayerPort.Beep(request)");
                            Decrement();
                        }
                    )
                );

                Decrement();
            }

            void OnBeepSuccess(dssp.DefaultSubmitResponseType response)
            {
                timer.SetTimerRequest request = new timer.SetTimerRequest();
                request.Interval = 1000;

                Increment();
                Activate(
                    ccr.Arbiter.Choice(
                        TimerPort.Wait(request),
                        OnWaitSuccess,
                        delegate(soap.Fault fault)
                        {
                            base.FaultHandler(fault, @"TimerPort.Wait(request)");
                            Decrement();
                        }
                    )
                );

                Decrement();
            }

            void OnWaitSuccess(dssp.DefaultSubmitResponseType response)
            {

                Increment();
                _mergeAlpha.Post(500);

                Decrement();
            }

            ccr.Port<int> _mergeAlpha = new ccr.Port<int>();

            ///////////////////////////////////////////////////////////////////
            // snippet.snippet.noop - snippet.snippet.expr - snippet.snippet.join
            ///////////////////////////////////////////////////////////////////

            void _mergeAlphaHandler(int message)
            {
                Increment();
                _joinAlphaPorts[0].Post(message);

                Decrement();
            }

            class JoinGamma
            {
                public JoinBeta Axis;
                public float Angle;

                public JoinGamma()
                {
                }

                public JoinGamma(object[] args)
                {
                    Axis = (JoinBeta)args[0];
                    Angle = (float)args[1];
                }

                public static explicit operator Microsoft.Robotics.PhysicalModel.Proxy.AxisAngle(JoinGamma join)
                {
                    Microsoft.Robotics.PhysicalModel.Proxy.AxisAngle request = new Microsoft.Robotics.PhysicalModel.Proxy.AxisAngle();

                    request.Axis = (Microsoft.Robotics.PhysicalModel.Proxy.Vector3)join.Axis;
                    request.Angle = join.Angle;
                    return request;
                }
            }

            ccr.Port<object>[] _joinGammaPorts = new ccr.Port<object>[2]{
                new ccr.Port<object>(),
                new ccr.Port<object>()
            };

            class JoinDelta
            {
                public string JointName;
                public string TargetPosition;
                public string TargetOrientation;

                public JoinDelta()
                {
                }

                public JoinDelta(object[] args)
                {
                    JointName = args[0].ToString();
                    TargetPosition = args[1].ToString();
                    TargetOrientation = args[2].ToString();
                }
            }

            ccr.Port<object>[] _joinDeltaPorts = new ccr.Port<object>[3]{
                new ccr.Port<object>(),
                new ccr.Port<object>(),
                new ccr.Port<object>()
            };

            class JoinAlpha
            {
                public int value;

                public JoinAlpha()
                {
                }

                public JoinAlpha(object[] args)
                {
                    value = (int)args[0];
                }
            }

            ccr.Port<object>[] _joinAlphaPorts = new ccr.Port<object>[1]{
                new ccr.Port<object>()
            };

            class JoinEpsilon
            {
                public string Synchronous;

                public JoinEpsilon()
                {
                }

                public JoinEpsilon(object[] args)
                {
                    Synchronous = args[0].ToString();
                }
            }

            ccr.Port<object>[] _joinEpsilonPorts = new ccr.Port<object>[1]{
                new ccr.Port<object>()
            };

            class JoinZeta
            {
                public string Interval;

                public JoinZeta()
                {
                }

                public JoinZeta(object[] args)
                {
                    Interval = args[0].ToString();
                }
            }

            ccr.Port<object>[] _joinZetaPorts = new ccr.Port<object>[1]{
                new ccr.Port<object>()
            };

            class JoinBeta
            {
                public int X;
                public int Y;
                public int Z;

                public JoinBeta()
                {
                }

                public JoinBeta(object[] args)
                {
                    X = (int)args[0];
                    Y = (int)args[1];
                    Z = (int)args[2];
                }

                public static explicit operator Microsoft.Robotics.PhysicalModel.Proxy.Vector3(JoinBeta join)
                {
                    Microsoft.Robotics.PhysicalModel.Proxy.Vector3 request = new Microsoft.Robotics.PhysicalModel.Proxy.Vector3();

                    request.X = (float)join.X;
                    request.Y = (float)join.Y;
                    request.Z = (float)join.Z;
                    return request;
                }
            }

            ccr.Port<object>[] _joinBetaPorts = new ccr.Port<object>[3]{
                new ccr.Port<object>(),
                new ccr.Port<object>(),
                new ccr.Port<object>()
            };

            ccr.Port<object>[] _secondJoinBetaPorts = new ccr.Port<object>[3]{
                new ccr.Port<object>(),
                new ccr.Port<object>(),
                new ccr.Port<object>()
            };
        }

        #endregion

        #region Handler utility base class

        class HandlerBase : ccr.CcrServiceBase
        {
            ccr.Port<ccr.EmptyValue> _complete = new ccr.Port<ccr.EmptyValue>();
            ccr.Port<ccr.EmptyValue> _shutdown = new ccr.Port<ccr.EmptyValue>();
            List<ccr.Port<object>[]> _joins = new List<ccr.Port<object>[]>();
            MoveJointService _service;
            int _count;

            public HandlerBase(MoveJointService service, ccr.DispatcherQueue queue)
                : base(queue)
            {
                _service = service;
            }

            protected MoveJointState State
            {
                get { return _service._state; }
            }

            bool _stateChanged;

            public bool StateChanged
            {
                get { return _stateChanged; }
                set { _stateChanged = value; }
            }

            protected sound.SoundOperations SoundPlayerPort
            {
                get { return _service._soundPlayerPort; }
            }

            protected timer.TimerOperations TimerPort
            {
                get { return _service._timerPort; }
            }

            protected articulatedarm.ArticulatedArmOperations LynxL6ArmPort
            {
                get { return _service._lynxL6ArmPort; }
            }

            public ccr.Port<ccr.EmptyValue> Complete
            {
                get { return _complete; }
            }

            protected void Increment()
            {
                System.Threading.Interlocked.Increment(ref _count);
            }

            protected void Decrement()
            {
                int offset = 0;

                foreach (ccr.Port<object>[] join in _joins)
                {
                    bool complete = true;
                    int joinCount = 0;

                    foreach (ccr.Port<object> sink in join)
                    {
                        if (sink.ItemCount != 0)
                        {
                            joinCount += sink.ItemCount;
                        }
                        else
                        {
                            complete = false;
                        }
                    }
                    if (complete == false)
                    {
                        offset += joinCount;
                    }
                }

                if (System.Threading.Interlocked.Decrement(ref _count) <= offset)
                {
                    _shutdown.Post(ccr.EmptyValue.SharedInstance);
                    _complete.Post(ccr.EmptyValue.SharedInstance);
                }
            }

            protected void Decrement(int count)
            {
                for(int i = 0; i < count; i++)
                {
                    Decrement();
                }
            }

            protected void RegisterJoin(ccr.Port<object>[] join)
            {
                _joins.Add(join);
            }

            protected ccr.ITask WaitUntilComplete()
            {
                return ccr.Arbiter.Receive(false, _shutdown, NullDelegate);
            }

            protected static void NullDelegate(ccr.EmptyValue token)
            {
            }

            protected void SendNotification<T>(object notification)
                where T : dssp.DsspOperation, new()
            {
                _service.SendNotification<T>(_service._subMgr, notification);
            }

            protected void FaultHandler(soap.Fault fault, string msg)
            {
                _service.LogError(null, msg, fault);
            }

            protected static string Stringize(object obj)
            {
                if (obj == null)
                {
                    return string.Empty;
                }
                else
                {
                    return obj.ToString();
                }
            }

            protected void UnhandledResponse(W3C.Soap.Fault fault)
            {
                _service.LogError("Unhandled fault response from partner service", fault);
                Decrement();
            }

            protected void UnhandledResponse<T>(T response)
            {
                Decrement();
            }

            #region Type Cast Functions

            protected static bool BoolCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToBoolean(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return false;
            }

            protected static byte ByteCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToByte(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static char CharCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToChar(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return '\0';
            }

            protected static decimal DecimalCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToDecimal(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static double DoubleCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToDouble(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static float FloatCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToSingle(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static int IntCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToInt32(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static long LongCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToInt64(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static sbyte SByteCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToSByte(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static short ShortCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToInt16(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static uint UIntCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToUInt32(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static ulong ULongCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToUInt64(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            protected static ushort UShortCast(object obj)
            {
                if (obj != null && obj is IConvertible)
                {
                    try
                    {
                        return ((IConvertible)obj).ToUInt16(System.Globalization.CultureInfo.InvariantCulture);
                    }
                    catch (FormatException)
                    {
                    }
                }
                return 0;
            }

            #endregion

            #region List Functions

            protected List<T> ListAdd<T>(List<T> list, T item)
            {
                List<T> output = new List<T>(list);
                output.Add(item);
                return output;
            }

            protected List<T> ListConcat<T>(List<T> head, List<T> tail)
            {
                List<T> output = new List<T>(head);
                output.AddRange(tail);
                return output;
            }

            protected int ListIndex<T>(List<T> list, T item)
            {
                return list.IndexOf(item);
            }

            protected List<T> ListRemove<T>(List<T> list, int index)
            {
                List<T> output = new List<T>(list);
                output.RemoveAt(index);
                return output;
            }

            protected List<T> ListReverse<T>(List<T> list)
            {
                List<T> output = new List<T>(list);
                output.Reverse();
                return output;
            }

            protected List<T> ListSort<T>(List<T> list)
            {
                List<T> output = new List<T>(list);
                output.Sort();
                return output;
            }

            protected List<T> ListInsert<T>(List<T> list, T item, int index)
            {
                List<T> output = new List<T>(list);
                output.Insert(index, item);
                return output;
            }

            #endregion
        }

        #endregion
    }
}
